#include <pybind11_catkin/pybind11/pybind11.h>
#include <pybind11_catkin/pybind11/numpy.h>
#include <iostream>
#include <Eigen/Dense>

namespace py = pybind11;


py::array_t<double> transformpoints(py::array_t<double> inputpoints,py::array_t<double> q, py::array_t<double> r) {
    // 获取输入数组的 buffer_info
    py::buffer_info info_pts = inputpoints.request();
    py::buffer_info info_q = q.request();
    py::buffer_info info_r = r.request();

    int rows = info_pts.shape[0];
    int cols = info_pts.shape[1]; // 3
    const double* ptr_pointcloud = static_cast<const double*>(info_pts.ptr);

    const double* ptr_q = static_cast<const double*>(info_q.ptr);
    const double* ptr_r = static_cast<const double*>(info_r.ptr);

    // 创建一个新的 NumPy 数组来存储乘积结果
    py::array_t<double> result_array({rows, cols});
    py::buffer_info buf_info_result = result_array.request();
    double* result_ptr = static_cast<double*>(buf_info_result.ptr);

    Eigen::Quaterniond q_eigen(ptr_q[0],ptr_q[1],ptr_q[2],ptr_q[3]);
    // std::cout<<"q: "<<q_eigen.coeffs().transpose()<<"\n";
    Eigen::Vector3d r_eigen(ptr_r[0],ptr_r[1],ptr_r[2]);

    for (size_t i = 0; i < rows; i++)
    {
        double x = ptr_pointcloud[i*cols];
        double y = ptr_pointcloud[i*cols + 1];
        double z = ptr_pointcloud[i*cols + 2];
        Eigen::Vector3d pt(x,y,z);
        pt = q_eigen * pt + r_eigen;

        result_ptr[i*cols] = pt[0];
        result_ptr[i*cols + 1] = pt[1];
        result_ptr[i*cols + 2] = pt[2];
    }

    return result_array;
}

void hello_from_cpp() {
    std::cout << "Hello from C++!" << std::endl;
}

PYBIND11_MODULE(libpy_pointcloud, m) {
    m.def("transformpoints", &transformpoints, "A function that transform a NumPy array.");
    m.def("hello_from_cpp", &hello_from_cpp, "A function that says hello from C++.");
}

